diff --git a/include/ignition/gazebo/components/JointTransmittedWrench.hh b/include/ignition/gazebo/components/JointTransmittedWrench.hh new file mode 100644 index 0000000000..10cef6a294 --- /dev/null +++ b/include/ignition/gazebo/components/JointTransmittedWrench.hh @@ -0,0 +1,56 @@ +/* + * 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 { + +namespace components +{ +/// \brief Joint Transmitted wrench in SI units (Nm for torque, N for 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 pair of 3D vectors representing +/// torque and force quantities expessed in a given frame and where the force is +/// applied at the origin of the frame. This is different from the Wrench used +/// in screw theory. +/// \note The value of force_offset in msgs::Wrench is ignored for this +/// component. The force is assumed to be applied at the origin of the joint +/// frame. +using JointTransmittedWrench = + Component; +IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.JointTransmittedWrench", + JointTransmittedWrench) +} // namespace components +} +} +} + +#endif diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 3e78e90a61..cf52a465c5 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -116,6 +116,7 @@ #include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/ParentLinkName.hh" #include "ignition/gazebo/components/ExternalWorldWrenchCmd.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" @@ -379,6 +380,19 @@ class ignition::gazebo::systems::PhysicsPrivate } return true; }}; + /// \brief msgs::Contacts equality comparison function. + public: std::function + wrenchEql{ + [](const msgs::Wrench &_a, const msgs::Wrench &_b) + { + return math::equal(_a.torque().x(), _b.torque().x(), 1e-6) && + math::equal(_a.torque().y(), _b.torque().y(), 1e-6) && + math::equal(_a.torque().z(), _b.torque().z(), 1e-6) && + + math::equal(_a.force().x(), _b.force().x(), 1e-6) && + math::equal(_a.force().y(), _b.force().y(), 1e-6) && + math::equal(_a.force().z(), _b.force().z(), 1e-6); + }}; /// \brief Environment variable which holds paths to look for engine plugins public: std::string pluginPathEnv = "IGN_GAZEBO_PHYSICS_ENGINE_PATH"; @@ -418,6 +432,12 @@ class ignition::gazebo::systems::PhysicsPrivate physics::DetachJointFeature, physics::SetJointTransformFromParentFeature>{}; + ////////////////////////////////////////////////// + // Joint transmitted wrench + /// \brief Feature list for getting joint transmitted wrenches. + public: struct JointGetTransmittedWrenchFeatureList : physics::FeatureList< + physics::GetJointTransmittedWrench>{}; + ////////////////////////////////////////////////// // Collisions @@ -561,7 +581,8 @@ class ignition::gazebo::systems::PhysicsPrivate physics::Joint, JointFeatureList, DetachableJointFeatureList, - JointVelocityCommandFeatureList + JointVelocityCommandFeatureList, + JointGetTransmittedWrenchFeatureList >; /// \brief A map between joint entity ids in the ECM to Joint Entities in @@ -2777,6 +2798,46 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, }); IGN_PROFILE_END(); + // Update joint transmitteds + _ecm.Each( + [&](const Entity &_entity, components::Joint *, + components::JointTransmittedWrench *_wrench) -> bool + { + auto jointPhys = + this->entityJointMap + .EntityCast(_entity); + if (jointPhys) + { + const auto &jointWrench = jointPhys->GetTransmittedWrench(); + + msgs::Wrench wrenchData; + msgs::Set(wrenchData.mutable_torque(), + math::eigen3::convert(jointWrench.torque)); + msgs::Set(wrenchData.mutable_force(), + math::eigen3::convert(jointWrench.force)); + const auto state = + _wrench->SetData(wrenchData, this->wrenchEql) + ? ComponentState::PeriodicChange + : ComponentState::NoChange; + _ecm.SetChanged(_entity, components::JointTransmittedWrench::typeId, + state); + } + else + { + static bool informed{false}; + if (!informed) + { + igndbg + << "Attempting to get joint transmitted wrenches, but the " + "physics engine doesn't support this feature. Values in the " + "JointTransmittedWrench component will not be meaningful." + << std::endl; + informed = true; + } + } + return true; + }); + // TODO(louise) Skip this if there are no collision features this->UpdateCollisions(_ecm); } diff --git a/test/integration/components.cc b/test/integration/components.cc index 2c18a49d92..e84ba44a42 100644 --- a/test/integration/components.cc +++ b/test/integration/components.cc @@ -18,6 +18,8 @@ #include #include +#include +#include #include @@ -48,6 +50,7 @@ #include "ignition/gazebo/components/Inertial.hh" #include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/JointAxis.hh" +#include "ignition/gazebo/components/JointTransmittedWrench.hh" #include "ignition/gazebo/components/JointType.hh" #include "ignition/gazebo/components/JointVelocity.hh" #include "ignition/gazebo/components/JointVelocityCmd.hh" @@ -1661,3 +1664,25 @@ TEST_F(ComponentsTest, ParticleEmitterCmd) EXPECT_EQ(comp1.Data().emitting().data(), comp3.Data().emitting().data()); EXPECT_EQ(comp1.Data().name(), comp3.Data().name()); } + +////////////////////////////////////////////////// +TEST_F(ComponentsTest, JointTransmittedWrench) +{ + msgs::Wrench wrench; + msgs::Set(wrench.mutable_torque(), {10, 20, 30}); + msgs::Set(wrench.mutable_force(), {1, 2, 3}); + + // // Create components. + auto comp1 = components::JointTransmittedWrench(wrench); + + // Stream operators. + std::ostringstream ostr; + comp1.Serialize(ostr); + + std::istringstream istr(ostr.str()); + components::JointTransmittedWrench comp2; + comp2.Deserialize(istr); + EXPECT_EQ(msgs::Convert(comp2.Data().force()), msgs::Convert(wrench.force())); + EXPECT_EQ(msgs::Convert(comp2.Data().torque()), + msgs::Convert(wrench.torque())); +} diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index 174ac27a12..38f4fdd07c 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -23,6 +23,7 @@ #include #include +#include #include #include #include @@ -44,6 +45,7 @@ #include "ignition/gazebo/components/Geometry.hh" #include "ignition/gazebo/components/Inertial.hh" #include "ignition/gazebo/components/Joint.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" @@ -56,6 +58,7 @@ #include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/PoseCmd.hh" #include "ignition/gazebo/components/Static.hh" #include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/World.hh" @@ -1662,3 +1665,124 @@ TEST_F(PhysicsSystemFixture, Heightmap) EXPECT_TRUE(checked); EXPECT_EQ(1000, maxIt); } + +///////////////////////////////////////////////// +// Joint force +TEST_F(PhysicsSystemFixture, JointTransmittedWrench) +{ + common::Console::SetVerbosity(4); + ignition::gazebo::ServerConfig serverConfig; + + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/joint_transmitted_wrench.sdf"; + + serverConfig.SetSdfFile(sdfFile); + + gazebo::Server server(serverConfig); + + server.SetUpdatePeriod(1us); + + // Create a system that records the poses of the links after physics + test::Relay testSystem; + + testSystem.OnPreUpdate( + [&](const gazebo::UpdateInfo &_info, gazebo::EntityComponentManager &_ecm) + { + if (_info.iterations == 1) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const components::Joint *) -> bool + { + _ecm.CreateComponent(_entity, + 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 &, + const gazebo::EntityComponentManager &_ecm) + { + 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, 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, msgs::Convert(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, msgs::Convert(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_transmitted_wrench.sdf b/test/worlds/joint_transmitted_wrench.sdf new file mode 100644 index 0000000000..48e43b2dbe --- /dev/null +++ b/test/worlds/joint_transmitted_wrench.sdf @@ -0,0 +1,162 @@ + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + 0 0 0.02 0 0 0 + + + 2.0 + + 0.166933 + 0.166933 + 0.333333 + + + + + + 1 1 0.04 + + + + + 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 + + + + + -0.5 0.5 -0.01 0 0 0 + + + 0.01 + + + + + + 0 0 0.03 0 0 0 + + 0.2 + + 0.0166933 + 0.0166933 + 0.0333333 + + + + + + 0.9 0.9 0.01 + + + + 0.8 0.2 0.2 1 + 0.8 0.2 0.2 1 + 0.8 0.2 0.2 1 + + + + + + 0.9 0.9 0.01 + + + + + + + base + sensor_plate + + + + + 0 0 0.5 0 0 0 + + + 10 + + + + + 0.25 + + + + 0.0 0.8 0.8 1 + 0.0 0.8 0.8 1 + 0.0 0.8 0.8 1 + + + + + + 0.25 + + + + + + + +