From 1eaf18c7f19ad9e823c8e1ca03196311e69b2892 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 20 Aug 2021 15:15:47 -0500 Subject: [PATCH] Populate JointConstraintWrench from physics Signed-off-by: Addisu Z. Taddese --- .../components/JointConstraintWrench.hh | 52 +++++++ .../gazebo/components/Serialization.hh | 38 +++++ src/systems/physics/Physics.cc | 43 +++++- test/integration/physics_system.cc | 60 ++++++++ test/worlds/joint_constraints.sdf | 137 ++++++++++++++++++ 5 files changed, 329 insertions(+), 1 deletion(-) create mode 100644 include/ignition/gazebo/components/JointConstraintWrench.hh create mode 100644 test/worlds/joint_constraints.sdf diff --git a/include/ignition/gazebo/components/JointConstraintWrench.hh b/include/ignition/gazebo/components/JointConstraintWrench.hh new file mode 100644 index 0000000000..9452c91646 --- /dev/null +++ b/include/ignition/gazebo/components/JointConstraintWrench.hh @@ -0,0 +1,52 @@ +/* + * 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/Serialization.hh b/include/ignition/gazebo/components/Serialization.hh index 5fe9999dce..e63e0e6131 100644 --- a/include/ignition/gazebo/components/Serialization.hh +++ b/include/ignition/gazebo/components/Serialization.hh @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -131,6 +132,43 @@ 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 6e675264b2..135296e0c2 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -109,6 +110,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/JointForceCmd.hh" #include "ignition/gazebo/components/PhysicsEnginePlugin.hh" #include "ignition/gazebo/components/Pose.hh" @@ -284,6 +286,12 @@ class ignition::gazebo::systems::PhysicsPrivate physics::DetachJointFeature, physics::SetJointTransformFromParentFeature>{}; + ////////////////////////////////////////////////// + // Joint constraint wrench + /// \brief Feature list for getting joint constraint wrenches. + public: struct JointGetConstraintWrenchFeatureList : physics::FeatureList< + physics::GetJointConstraintWrench>{}; + ////////////////////////////////////////////////// // Collisions @@ -397,7 +405,8 @@ class ignition::gazebo::systems::PhysicsPrivate physics::Joint, JointFeatureList, DetachableJointFeatureList, - JointVelocityCommandFeatureList + JointVelocityCommandFeatureList, + JointGetConstraintWrenchFeatureList >; /// \brief A map between joint entity ids in the ECM to Joint Entities in @@ -2189,6 +2198,38 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm) return true; }); + // Update joint constraints + _ecm.Each( + [&](const Entity &_entity, components::Joint *, + components::JointConstraintWrench *_wrench) -> bool + { + auto jointPhys = + this->entityJointMap + .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); + } + else + { + static bool informed{false}; + if (!informed) + { + igndbg + << "Attempting to get joint constraint wrenches, but the " + "physics engine doesn't support this feature. Values in the " + "JointConstraintWrench 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/physics_system.cc b/test/integration/physics_system.cc index f6d31c8436..0b51604eb2 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -41,6 +41,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/JointPosition.hh" #include "ignition/gazebo/components/JointPositionReset.hh" #include "ignition/gazebo/components/JointVelocity.hh" @@ -981,3 +982,62 @@ TEST_F(PhysicsSystemFixture, NestedModel) EXPECT_NE(parents.end(), parentIt); EXPECT_EQ("model_01", parentIt->second); } + +///////////////////////////////////////////////// +// Joint force +TEST_F(PhysicsSystemFixture, JointConstraintWrench) +{ + common::Console::SetVerbosity(4); + ignition::gazebo::ServerConfig serverConfig; + + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/joint_constraints.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::JointConstraintWrench()); + return true; + }); + } + }); + + testSystem.OnPostUpdate( + [&](const gazebo::UpdateInfo &_info, + 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; + }); + }); + server.AddSystem(testSystem.systemPtr); + server.Run(true, 1800, false); + SUCCEED(); +} diff --git a/test/worlds/joint_constraints.sdf b/test/worlds/joint_constraints.sdf new file mode 100644 index 0000000000..24478eb6bc --- /dev/null +++ b/test/worlds/joint_constraints.sdf @@ -0,0 +1,137 @@ + + + + + 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 + + + + + + + 1 1 0.04 + + + + + + 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.5 0.5 0.5 + + + + 0.0 0.8 0.8 1 + 0.0 0.8 0.8 1 + 0.0 0.8 0.8 1 + + + + + + 0.5 0.5 0.5 + + + + + + + +