diff --git a/include/ignition/gazebo/Joint.hh b/include/ignition/gazebo/Joint.hh new file mode 100644 index 0000000000..f55a79d768 --- /dev/null +++ b/include/ignition/gazebo/Joint.hh @@ -0,0 +1,276 @@ +/* + * Copyright (C) 2023 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_JOINT_HH_ +#define IGNITION_GAZEBO_JOINT_HH_ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +#include "ignition/gazebo/config.hh" +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/Export.hh" +#include "ignition/gazebo/Types.hh" + +namespace ignition +{ + namespace gazebo + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + // + /// \class Joint Joint.hh ignition/gazebo/Joint.hh + /// \brief This class provides wrappers around entities and components + /// which are more convenient and straight-forward to use than dealing + /// with the `EntityComponentManager` directly. + /// All the functions provided here are meant to be used with a joint + /// entity. + /// + /// For example, given a joint's entity, find the value of its + /// name component, one could use the entity-component manager (`ecm`) + /// directly as follows: + /// + /// std::string name = ecm.Component(entity)->Data(); + /// + /// Using this class however, the same information can be obtained with + /// a simpler function call: + /// + /// Joint joint(entity); + /// std::string name = joint.Name(ecm); + /// + class IGNITION_GAZEBO_VISIBLE Joint + { + /// \brief Constructor + /// \param[in] _entity Joint entity + public: explicit Joint(gazebo::Entity _entity = kNullEntity); + + /// \brief Get the entity which this Joint is related to. + /// \return Joint entity. + public: gazebo::Entity Entity() const; + + /// \brief Reset Entity to a new one + /// \param[in] _newEntity New joint entity. + public: void ResetEntity(gazebo::Entity _newEntity); + + /// \brief Check whether this joint correctly refers to an entity that + /// has a components::Joint. + /// \param[in] _ecm Entity-component manager. + /// \return True if it's a valid joint in the manager. + public: bool Valid(const EntityComponentManager &_ecm) const; + + /// \brief Get the joint's unscoped name. + /// \param[in] _ecm Entity-component manager. + /// \return Joint's name or nullopt if the entity does not have a + /// components::Name component + public: std::optional Name( + const EntityComponentManager &_ecm) const; + + /// \brief Get the parent link name + /// \param[in] _ecm Entity-component manager. + /// \return Parent link name or nullopt if the entity does not have a + /// components::ParentLinkName component. + public: std::optional ParentLinkName( + const EntityComponentManager &_ecm) const; + + /// \brief Get the child link name + /// \param[in] _ecm Entity-component manager. + /// \return Child link name or nullopt if the entity does not have a + /// components::ChildLinkName component. + public: std::optional ChildLinkName( + const EntityComponentManager &_ecm) const; + + /// \brief Get the pose of the joint + /// \param[in] _ecm Entity-component manager. + /// \return Pose of the joint or nullopt if the entity does not + /// have a components::Pose component. + public: std::optional Pose( + const EntityComponentManager &_ecm) const; + + /// \brief Get the thread pitch of the joint + /// \param[in] _ecm Entity-component manager. + /// \return Thread pitch of the joint or nullopt if the entity does not + /// have a components::ThreadPitch component. + public: std::optional ThreadPitch( + const EntityComponentManager &_ecm) const; + + /// \brief Get the joint axis + /// \param[in] _ecm Entity-component manager. + /// \return Axis of the joint or nullopt if the entity does not + /// have a components::JointAxis or components::JointAxis2 component. + public: std::optional> Axis( + const EntityComponentManager &_ecm) const; + + /// \brief Get the joint type + /// \param[in] _ecm Entity-component manager. + /// \return Type of the joint or nullopt if the entity does not + /// have a components::JointType component. + public: std::optional Type( + const EntityComponentManager &_ecm) const; + + /// \brief Get the ID of a sensor entity which is an immediate child of + /// this joint. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _name Sensor name. + /// \return Sensor entity. + public: gazebo::Entity SensorByName(const EntityComponentManager &_ecm, + const std::string &_name) const; + + /// \brief Get all sensors which are immediate children of this joint. + /// \param[in] _ecm Entity-component manager. + /// \return All sensors in this joint. + public: std::vector Sensors( + const EntityComponentManager &_ecm) const; + + /// \brief Get the number of sensors which are immediate children of this + /// joint. + /// \param[in] _ecm Entity-component manager. + /// \return Number of sensors in this joint. + public: uint64_t SensorCount(const EntityComponentManager &_ecm) const; + + /// \brief Set velocity on this joint. Only applied if no forces are set + /// \param[in] _ecm Entity Component manager. + /// \param[in] _velocities Joint velocities commands (target velocities). + /// This is different from ResetVelocity in that this does not modify the + /// internal state of the joint. Instead, the physics engine is expected + /// to compute the necessary joint torque for the commanded velocity and + /// apply it in the next simulation step. The vector of velocities should + /// have the same size as the degrees of freedom of the joint. + public: void SetVelocity(EntityComponentManager &_ecm, + const std::vector &_velocities); + + /// \brief Set force on this joint. If both forces and velocities are set, + /// only forces are applied + /// \param[in] _ecm Entity Component manager. + /// \param[in] _forces Joint force or torque commands (target forces + /// or toruqes). The vector of forces should have the same size as the + /// degrees of freedom of the joint. + public: void SetForce(EntityComponentManager &_ecm, + const std::vector &_forces); + + /// \brief Set the velocity limits on a joint axis. + /// \param[in] _ecm Entity Component manager. + /// \param[in] _limits Joint limits to set to. The X() component of the + /// Vector2 specifies the minimum velocity limit, the Y() component stands + /// for maximum limit. The vector of limits should have the same size as + /// the degrees of freedom of the joint. + public: void SetVelocityLimits(EntityComponentManager &_ecm, + const std::vector &_limits); + + /// \brief Set the effort limits on a joint axis. + /// \param[in] _ecm Entity Component manager. + /// \param[in] _limits Joint limits to set to. The X() component of the + /// Vector2 specifies the minimum effort limit, the Y() component stands + /// for maximum limit. The vector of limits should have the same size as + /// the degrees of freedom of the joint. + public: void SetEffortLimits(EntityComponentManager &_ecm, + const std::vector &_limits); + + /// \brief Set the position limits on a joint axis. + /// \param[in] _ecm Entity Component manager. + /// \param[in] _limits Joint limits to set to. The X() component of the + /// Vector2 specifies the minimum position limit, the Y() component stands + /// for maximum limit. The vector of limits should have the same size as + /// the degrees of freedom of the joint. + public: void SetPositionLimits(EntityComponentManager &_ecm, + const std::vector &_limits); + + /// \brief Reset the joint positions + /// \param[in] _ecm Entity Component manager. + /// \param[in] _positions Joint positions to reset to. + /// The vector of positions should have the same size as the degrees of + /// freedom of the joint. + public: void ResetPosition(EntityComponentManager &_ecm, + const std::vector &_positions); + + /// \brief Reset the joint velocities + /// \param[in] _ecm Entity Component manager. + /// \param[in] _velocities Joint velocities to reset to. This is different + /// from SetVelocity as this modifies the internal state of the joint. + /// The vector of velocities should have the same size as the degrees of + /// freedom of the joint. + public: void ResetVelocity(EntityComponentManager &_ecm, + const std::vector &_velocities); + + /// \brief By default, Gazebo will not report velocities for a joint, so + /// functions like `Velocity` will return nullopt. This + /// function can be used to enable joint velocity check. + /// \param[in] _ecm Mutable reference to the ECM. + /// \param[in] _enable True to enable checks, false to disable. Defaults + /// to true. + public: void EnableVelocityCheck(EntityComponentManager &_ecm, + bool _enable = true) const; + + /// \brief By default, Gazebo will not report positions for a joint, so + /// functions like `Position` will return nullopt. This + /// function can be used to enable joint position check. + /// \param[in] _ecm Mutable reference to the ECM. + /// \param[in] _enable True to enable checks, false to disable. Defaults + /// to true. + public: void EnablePositionCheck(EntityComponentManager &_ecm, + bool _enable = true) const; + + /// \brief By default, Gazebo will not report transmitted wrench for a + /// joint, so functions like `TransmittedWrench` will return nullopt. This + /// function can be used to enable joint transmitted wrench check. + /// \param[in] _ecm Mutable reference to the ECM. + /// \param[in] _enable True to enable checks, false to disable. Defaults + /// to true. + public: void EnableTransmittedWrenchCheck(EntityComponentManager &_ecm, + bool _enable = true) const; + + /// \brief Get the velocity of the joint + /// \param[in] _ecm Entity-component manager. + /// \return Velocity of the joint or nullopt if velocity check + /// is not enabled. + /// \sa EnableVelocityCheck + public: std::optional> Velocity( + const EntityComponentManager &_ecm) const; + + /// \brief Get the position of the joint + /// \param[in] _ecm Entity-component manager. + /// \return Position of the joint or nullopt if position check + /// is not enabled. + /// \sa EnablePositionCheck + public: std::optional> Position( + const EntityComponentManager &_ecm) const; + + /// \brief Get the position of the joint + /// \param[in] _ecm Entity-component manager. + /// \return Transmitted wrench of the joint or nullopt if transmitted + /// wrench check is not enabled. + /// \sa EnableTransmittedWrenchCheck + public: std::optional> TransmittedWrench( + const EntityComponentManager &_ecm) const; + + /// \brief Private data pointer. + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} +#endif diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index b88261e28d..97b0a48238 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -48,6 +48,7 @@ set (sources Conversions.cc ComponentFactory.cc EntityComponentManager.cc + Joint.cc LevelManager.cc Link.cc Model.cc @@ -79,6 +80,7 @@ set (gtest_sources Conversions_TEST.cc EntityComponentManager_TEST.cc EventManager_TEST.cc + Joint_TEST.cc Link_TEST.cc Model_TEST.cc Primitives_TEST.cc diff --git a/src/Joint.cc b/src/Joint.cc new file mode 100644 index 0000000000..a57e531860 --- /dev/null +++ b/src/Joint.cc @@ -0,0 +1,374 @@ +/* + * Copyright (C) 2023 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. + * + */ + +#include "ignition/gazebo/components/ChildLinkName.hh" +#include "ignition/gazebo/components/Joint.hh" +#include "ignition/gazebo/components/JointAxis.hh" +#include "ignition/gazebo/components/JointEffortLimitsCmd.hh" +#include "ignition/gazebo/components/JointForceCmd.hh" +#include "ignition/gazebo/components/JointPosition.hh" +#include "ignition/gazebo/components/JointPositionLimitsCmd.hh" +#include "ignition/gazebo/components/JointPositionReset.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" +#include "ignition/gazebo/components/JointVelocityLimitsCmd.hh" +#include "ignition/gazebo/components/JointVelocityReset.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/Sensor.hh" +#include "ignition/gazebo/components/ThreadPitch.hh" + +#include "ignition/gazebo/Joint.hh" +#include "ignition/gazebo/Util.hh" + +using namespace ignition; +using namespace gazebo; + + +class ignition::gazebo::Joint::Implementation +{ + /// \brief Id of joint entity. + public: gazebo::Entity id{kNullEntity}; +}; + +////////////////////////////////////////////////// +Joint::Joint(gazebo::Entity _entity) + : dataPtr(utils::MakeImpl()) +{ + this->dataPtr->id = _entity; +} + +////////////////////////////////////////////////// +gazebo::Entity Joint::Entity() const +{ + return this->dataPtr->id; +} + +////////////////////////////////////////////////// +void Joint::ResetEntity(gazebo::Entity _newEntity) +{ + this->dataPtr->id = _newEntity; +} + +////////////////////////////////////////////////// +bool Joint::Valid(const EntityComponentManager &_ecm) const +{ + return nullptr != _ecm.Component(this->dataPtr->id); +} + +////////////////////////////////////////////////// +std::optional Joint::Name(const EntityComponentManager &_ecm) const +{ + return _ecm.ComponentData(this->dataPtr->id); +} + +////////////////////////////////////////////////// +std::optional Joint::ParentLinkName( + const EntityComponentManager &_ecm) const +{ + auto parent = _ecm.Component(this->dataPtr->id); + + if (!parent) + return std::nullopt; + + return std::optional(parent->Data()); +} + +////////////////////////////////////////////////// +std::optional Joint::ChildLinkName( + const EntityComponentManager &_ecm) const +{ + auto child = _ecm.Component(this->dataPtr->id); + + if (!child) + return std::nullopt; + + return std::optional(child->Data()); +} + +////////////////////////////////////////////////// +std::optional Joint::Pose( + const EntityComponentManager &_ecm) const +{ + auto pose = _ecm.Component(this->dataPtr->id); + + if (!pose) + return std::nullopt; + + return std::optional(pose->Data()); +} + +////////////////////////////////////////////////// +std::optional Joint::ThreadPitch( + const EntityComponentManager &_ecm) const +{ + auto threadPitch = _ecm.Component(this->dataPtr->id); + + if (!threadPitch) + return std::nullopt; + + return std::optional(threadPitch->Data()); +} + +////////////////////////////////////////////////// +std::optional> Joint::Axis( + const EntityComponentManager &_ecm) const +{ + auto axis = _ecm.Component(this->dataPtr->id); + if (!axis) + return std::nullopt; + + std::vector axisVec{axis->Data()}; + + auto axis2 = _ecm.Component(this->dataPtr->id); + if (axis2) + axisVec.push_back(axis2->Data()); + + return std::optional>(axisVec); +} + +////////////////////////////////////////////////// +std::optional Joint::Type( + const EntityComponentManager &_ecm) const +{ + auto jointType = _ecm.Component(this->dataPtr->id); + + if (!jointType) + return std::nullopt; + + return std::optional(jointType->Data()); +} + +////////////////////////////////////////////////// +Entity Joint::SensorByName(const EntityComponentManager &_ecm, + const std::string &_name) const +{ + return _ecm.EntityByComponents( + components::ParentEntity(this->dataPtr->id), + components::Name(_name), + components::Sensor()); +} + +////////////////////////////////////////////////// +std::vector Joint::Sensors(const EntityComponentManager &_ecm) const +{ + return _ecm.EntitiesByComponents( + components::ParentEntity(this->dataPtr->id), + components::Sensor()); +} + +////////////////////////////////////////////////// +uint64_t Joint::SensorCount(const EntityComponentManager &_ecm) const +{ + return this->Sensors(_ecm).size(); +} + +////////////////////////////////////////////////// +void Joint::SetVelocity(EntityComponentManager &_ecm, + const std::vector &_velocities) +{ + auto jointVelocityCmd = + _ecm.Component(this->dataPtr->id); + + if (!jointVelocityCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::JointVelocityCmd(_velocities)); + } + else + { + jointVelocityCmd->Data() = _velocities; + } +} + +////////////////////////////////////////////////// +void Joint::SetForce(EntityComponentManager &_ecm, + const std::vector &_forces) +{ + auto jointForceCmd = + _ecm.Component(this->dataPtr->id); + + if (!jointForceCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::JointForceCmd(_forces)); + } + else + { + jointForceCmd->Data() = _forces; + } +} + +////////////////////////////////////////////////// +void Joint::SetVelocityLimits(EntityComponentManager &_ecm, + const std::vector &_limits) +{ + auto jointVelocityLimitsCmd = + _ecm.Component(this->dataPtr->id); + + if (!jointVelocityLimitsCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::JointVelocityLimitsCmd(_limits)); + } + else + { + jointVelocityLimitsCmd->Data() = _limits; + } +} + +////////////////////////////////////////////////// +void Joint::SetEffortLimits(EntityComponentManager &_ecm, + const std::vector &_limits) +{ + auto jointEffortLimitsCmd = + _ecm.Component(this->dataPtr->id); + + if (!jointEffortLimitsCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::JointEffortLimitsCmd(_limits)); + } + else + { + jointEffortLimitsCmd->Data() = _limits; + } +} + +////////////////////////////////////////////////// +void Joint::SetPositionLimits(EntityComponentManager &_ecm, + const std::vector &_limits) +{ + auto jointPosLimitsCmd = + _ecm.Component(this->dataPtr->id); + + if (!jointPosLimitsCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::JointPositionLimitsCmd(_limits)); + } + else + { + jointPosLimitsCmd->Data() = _limits; + } +} + +////////////////////////////////////////////////// +void Joint::ResetVelocity(EntityComponentManager &_ecm, + const std::vector &_velocities) +{ + auto jointVelocityReset = + _ecm.Component(this->dataPtr->id); + + if (!jointVelocityReset) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::JointVelocityReset(_velocities)); + } + else + { + jointVelocityReset->Data() = _velocities; + } +} + +////////////////////////////////////////////////// +void Joint::ResetPosition(EntityComponentManager &_ecm, + const std::vector &_positions) +{ + auto jointPositionReset = + _ecm.Component(this->dataPtr->id); + + if (!jointPositionReset) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::JointPositionReset(_positions)); + } + else + { + jointPositionReset->Data() = _positions; + } +} + +////////////////////////////////////////////////// +void Joint::EnableVelocityCheck(EntityComponentManager &_ecm, bool _enable) + const +{ + enableComponent(_ecm, this->dataPtr->id, + _enable); +} + +////////////////////////////////////////////////// +void Joint::EnablePositionCheck(EntityComponentManager &_ecm, bool _enable) + const +{ + enableComponent(_ecm, this->dataPtr->id, + _enable); +} + +////////////////////////////////////////////////// +void Joint::EnableTransmittedWrenchCheck(EntityComponentManager &_ecm, + bool _enable) const +{ + enableComponent(_ecm, this->dataPtr->id, + _enable); +} + +////////////////////////////////////////////////// +std::optional> Joint::Velocity( + const EntityComponentManager &_ecm) const +{ + return _ecm.ComponentData( + this->dataPtr->id); +} + +////////////////////////////////////////////////// +std::optional> Joint::Position( + const EntityComponentManager &_ecm) const +{ + return _ecm.ComponentData( + this->dataPtr->id); +} + +////////////////////////////////////////////////// +std::optional> Joint::TransmittedWrench( + const EntityComponentManager &_ecm) const +{ + // TransmittedWrench components contains one wrench msg value + // instead of a vector like JointPosition and JointVelocity + // components. + // todo(anyone) change JointTransmittedWrench to store a vector + // of wrench msgs? + // We provide an API that returns a vector which is consistent + // with Velocity and Position accessor functions + auto comp = _ecm.Component( + this->dataPtr->id); + if (!comp) + return std::nullopt; + std::vector wrenchVec{comp->Data()}; + return wrenchVec; +} diff --git a/src/Joint_TEST.cc b/src/Joint_TEST.cc new file mode 100644 index 0000000000..1dc8e0e56a --- /dev/null +++ b/src/Joint_TEST.cc @@ -0,0 +1,138 @@ +/* + * Copyright (C) 2023 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. + * +*/ + +#include + +#include + +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/Joint.hh" +#include "ignition/gazebo/components/Joint.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/Sensor.hh" + +///////////////////////////////////////////////// +TEST(JointTest, Constructor) +{ + ignition::gazebo::Joint jointNull; + EXPECT_EQ(ignition::gazebo::kNullEntity, jointNull.Entity()); + + ignition::gazebo::Entity id(3); + ignition::gazebo::Joint joint(id); + + EXPECT_EQ(id, joint.Entity()); +} + +///////////////////////////////////////////////// +TEST(JointTest, CopyConstructor) +{ + ignition::gazebo::Entity id(3); + ignition::gazebo::Joint joint(id); + + // Marked nolint because we are specifically testing copy + // constructor here (clang wants unnecessary copies removed) + ignition::gazebo::Joint jointCopy(joint); // NOLINT + EXPECT_EQ(joint.Entity(), jointCopy.Entity()); +} + +///////////////////////////////////////////////// +TEST(JointTest, CopyAssignmentOperator) +{ + ignition::gazebo::Entity id(3); + ignition::gazebo::Joint joint(id); + + ignition::gazebo::Joint jointCopy; + jointCopy = joint; + EXPECT_EQ(joint.Entity(), jointCopy.Entity()); +} + +///////////////////////////////////////////////// +TEST(JointTest, MoveConstructor) +{ + ignition::gazebo::Entity id(3); + ignition::gazebo::Joint joint(id); + + ignition::gazebo::Joint jointMoved(std::move(joint)); + EXPECT_EQ(id, jointMoved.Entity()); +} + +///////////////////////////////////////////////// +TEST(JointTest, MoveAssignmentOperator) +{ + ignition::gazebo::Entity id(3); + ignition::gazebo::Joint joint(id); + + ignition::gazebo::Joint jointMoved; + jointMoved = std::move(joint); + EXPECT_EQ(id, jointMoved.Entity()); +} + +///////////////////////////////////////////////// +TEST(JointTest, Sensors) +{ + // jointA + // - sensorAA + // - sensorAB + // + // jointC + + ignition::gazebo::EntityComponentManager ecm; + + // Joint A + auto jointAEntity = ecm.CreateEntity(); + ecm.CreateComponent(jointAEntity, ignition::gazebo::components::Joint()); + ecm.CreateComponent(jointAEntity, + ignition::gazebo::components::Name("jointA_name")); + + // Sensor AA - Child of Joint A + auto sensorAAEntity = ecm.CreateEntity(); + ecm.CreateComponent(sensorAAEntity, ignition::gazebo::components::Sensor()); + ecm.CreateComponent(sensorAAEntity, + ignition::gazebo::components::Name("sensorAA_name")); + ecm.CreateComponent(sensorAAEntity, + ignition::gazebo::components::ParentEntity(jointAEntity)); + + // Sensor AB - Child of Joint A + auto sensorABEntity = ecm.CreateEntity(); + ecm.CreateComponent(sensorABEntity, ignition::gazebo::components::Sensor()); + ecm.CreateComponent(sensorABEntity, + ignition::gazebo::components::Name("sensorAB_name")); + ecm.CreateComponent(sensorABEntity, + ignition::gazebo::components::ParentEntity(jointAEntity)); + + // Joint C + auto jointCEntity = ecm.CreateEntity(); + ecm.CreateComponent(jointCEntity, ignition::gazebo::components::Joint()); + ecm.CreateComponent(jointCEntity, + ignition::gazebo::components::Name("jointC_name")); + + std::size_t foundSensors = 0; + + ignition::gazebo::Joint jointA(jointAEntity); + auto sensors = jointA.Sensors(ecm); + EXPECT_EQ(2u, sensors.size()); + for (const auto &sensor : sensors) + { + if (sensor == sensorAAEntity || sensor == sensorABEntity) + foundSensors++; + } + EXPECT_EQ(foundSensors, sensors.size()); + + ignition::gazebo::Joint jointC(jointCEntity); + EXPECT_EQ(0u, jointC.Sensors(ecm).size()); +} diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index d5776ad20f..8a5c61814d 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -27,6 +27,7 @@ set(tests hydrodynamics.cc hydrodynamics_flags.cc imu_system.cc + joint.cc joint_controller_system.cc joint_position_controller_system.cc joint_state_publisher_system.cc diff --git a/test/integration/joint.cc b/test/integration/joint.cc new file mode 100644 index 0000000000..91b3440365 --- /dev/null +++ b/test/integration/joint.cc @@ -0,0 +1,572 @@ +/* + * Copyright (C) 2023 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. + * +*/ + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "../helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace gazebo; + +class JointIntegrationTest : public InternalFixture<::testing::Test> +{ +}; + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, Valid) +{ + EntityComponentManager ecm; + + // No ID + { + Joint joint; + EXPECT_FALSE(joint.Valid(ecm)); + } + + // Missing joint component + { + auto id = ecm.CreateEntity(); + Joint joint(id); + EXPECT_FALSE(joint.Valid(ecm)); + } + + // Valid + { + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Joint()); + + Joint joint(id); + EXPECT_TRUE(joint.Valid(ecm)); + } +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, Name) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Joint()); + + Joint joint(id); + + // No name + EXPECT_EQ(std::nullopt, joint.Name(ecm)); + + // Add name + ecm.CreateComponent(id, components::Name("joint_name")); + EXPECT_EQ("joint_name", joint.Name(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, JointNames) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Joint()); + + Joint joint(id); + + // No parent or child joint names + EXPECT_EQ(std::nullopt, joint.ParentLinkName(ecm)); + EXPECT_EQ(std::nullopt, joint.ChildLinkName(ecm)); + + // Add parent joint name + ecm.CreateComponent(id, + components::ParentLinkName("parent_joint_name")); + EXPECT_EQ("parent_joint_name", joint.ParentLinkName(ecm)); + + // Add child joint name + ecm.CreateComponent(id, + components::ChildLinkName("child_joint_name")); + EXPECT_EQ("child_joint_name", joint.ChildLinkName(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, Pose) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Joint()); + + Joint joint(id); + + // No pose + EXPECT_EQ(std::nullopt, joint.Pose(ecm)); + + // Add pose + math::Pose3d pose(1, 2, 3, 0.1, 0.2, 0.3); + ecm.CreateComponent(id, + components::Pose(pose)); + EXPECT_EQ(pose, joint.Pose(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, Type) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Joint()); + + Joint joint(id); + + // No joint type + EXPECT_EQ(std::nullopt, joint.Type(ecm)); + + // Add type + sdf::JointType jointType = sdf::JointType::PRISMATIC; + ecm.CreateComponent(id, + components::JointType(jointType)); + EXPECT_EQ(jointType, joint.Type(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, Axis) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Joint()); + + Joint joint(id); + + // No joint axis + EXPECT_EQ(std::nullopt, joint.Axis(ecm)); + + // Add axis + sdf::JointAxis jointAxis; + auto errors = jointAxis.SetXyz(math::Vector3d(0, 1, 1)); + EXPECT_TRUE(errors.empty()); + ecm.CreateComponent(id, + components::JointAxis(jointAxis)); + EXPECT_EQ(jointAxis.Xyz(), (*joint.Axis(ecm))[0].Xyz()); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, ThreadPitch) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Joint()); + + Joint joint(id); + + // No name + EXPECT_EQ(std::nullopt, joint.ThreadPitch(ecm)); + + // Add name + ecm.CreateComponent(id, + components::ThreadPitch(1.23)); + EXPECT_DOUBLE_EQ(1.23, *joint.ThreadPitch(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, SensorByName) +{ + EntityComponentManager ecm; + + // Joint + auto eJoint = ecm.CreateEntity(); + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + EXPECT_EQ(0u, joint.SensorCount(ecm)); + + // Sensor + auto eSensor = ecm.CreateEntity(); + ecm.CreateComponent(eSensor, components::Sensor()); + ecm.CreateComponent(eSensor, + components::ParentEntity(eJoint)); + ecm.CreateComponent(eSensor, + components::Name("sensor_name")); + + // Check joint + EXPECT_EQ(eSensor, joint.SensorByName(ecm, "sensor_name")); + EXPECT_EQ(1u, joint.SensorCount(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, SetVelocity) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // No JointVelocityCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eJoint)); + + std::vector velCmd{1}; + joint.SetVelocity(ecm, velCmd); + + // velocity cmd should exist + EXPECT_NE(nullptr, ecm.Component(eJoint)); + EXPECT_EQ(velCmd, + ecm.Component(eJoint)->Data()); + + // Make sure the velocity cmd is updated + std::vector velCmd2{0}; + joint.SetVelocity(ecm, velCmd2); + EXPECT_EQ(velCmd2, + ecm.Component(eJoint)->Data()); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, SetForce) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // No JointForceCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eJoint)); + + std::vector forceCmd{10}; + joint.SetForce(ecm, forceCmd); + + // velocity cmd should exist + EXPECT_NE(nullptr, ecm.Component(eJoint)); + EXPECT_EQ(forceCmd, + ecm.Component(eJoint)->Data()); + + // Make sure the force cmd is updated + std::vector forceCmd2{1}; + joint.SetForce(ecm, forceCmd2); + EXPECT_EQ(forceCmd2, + ecm.Component(eJoint)->Data()); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, SetVelocityLimits) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // No JointVelocityLimitsCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eJoint)); + + std::vector velLimitsCmd{math::Vector2d(0.1, 1.1)}; + joint.SetVelocityLimits(ecm, velLimitsCmd); + + // velocity limits cmd should exist + EXPECT_NE(nullptr, ecm.Component(eJoint)); + EXPECT_EQ(velLimitsCmd, + ecm.Component(eJoint)->Data()); + + // Make sure the velocity limits cmd is updated + std::vector velLimitsCmd2{math::Vector2d(-0.2, 2.4)}; + joint.SetVelocityLimits(ecm, velLimitsCmd2); + EXPECT_EQ(velLimitsCmd2, + ecm.Component(eJoint)->Data()); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, SetEffortLimits) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // No JointEffortLimitsCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eJoint)); + + std::vector effortLimitsCmd{math::Vector2d(9, 9.9)}; + joint.SetEffortLimits(ecm, effortLimitsCmd); + + // effort limits cmd should exist + EXPECT_NE(nullptr, ecm.Component(eJoint)); + EXPECT_EQ(effortLimitsCmd, + ecm.Component(eJoint)->Data()); + + // Make sure the effort limits cmd is updated + std::vector effortLimitsCmd2{math::Vector2d(5.2, 5.4)}; + joint.SetEffortLimits(ecm, effortLimitsCmd2); + EXPECT_EQ(effortLimitsCmd2, + ecm.Component(eJoint)->Data()); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, SetPositionLimits) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // No JointPositionLimitsCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eJoint)); + + std::vector positionLimitsCmd{math::Vector2d(-0.1, 0.1)}; + joint.SetPositionLimits(ecm, positionLimitsCmd); + + // position limits cmd should exist + EXPECT_NE(nullptr, ecm.Component(eJoint)); + EXPECT_EQ(positionLimitsCmd, + ecm.Component(eJoint)->Data()); + + // Make sure the position limits cmd is updated + std::vector positionLimitsCmd2{math::Vector2d(-0.2, 0.4)}; + joint.SetPositionLimits(ecm, positionLimitsCmd2); + EXPECT_EQ(positionLimitsCmd2, + ecm.Component(eJoint)->Data()); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, ResetVelocity) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // No JointVelocityReset should exist by default + EXPECT_EQ(nullptr, ecm.Component(eJoint)); + + std::vector vel{1}; + joint.ResetVelocity(ecm, vel); + + // velocity reset should exist + EXPECT_NE(nullptr, ecm.Component(eJoint)); + EXPECT_EQ(vel, + ecm.Component(eJoint)->Data()); + + // Make sure the velocity reset is updated + std::vector vel2{0}; + joint.ResetVelocity(ecm, vel2); + EXPECT_EQ(vel2, + ecm.Component(eJoint)->Data()); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, ResetPosition) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // No JointPositionReset should exist by default + EXPECT_EQ(nullptr, ecm.Component(eJoint)); + + std::vector pos{1}; + joint.ResetPosition(ecm, pos); + + // position reset should exist + EXPECT_NE(nullptr, ecm.Component(eJoint)); + EXPECT_EQ(pos, + ecm.Component(eJoint)->Data()); + + // Make sure the position reset is updated + std::vector pos2{0}; + joint.ResetPosition(ecm, pos2); + EXPECT_EQ(pos2, + ecm.Component(eJoint)->Data()); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, Velocity) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // Before enabling, velocity should return nullopt + EXPECT_EQ(std::nullopt, joint.Velocity(ecm)); + + // After enabling, velocity should return default values + joint.EnableVelocityCheck(ecm); + + EXPECT_NE(nullptr, ecm.Component(eJoint)); + + std::vector velOut = *joint.Velocity(ecm); + EXPECT_TRUE(velOut.empty()); + + // With custom velocities + std::vector vel{0.3, 0.4}; + ecm.SetComponentData(eJoint, vel); + velOut = *joint.Velocity(ecm); + EXPECT_EQ(2u, velOut.size()); + EXPECT_DOUBLE_EQ(vel[0], velOut[0]); + EXPECT_DOUBLE_EQ(vel[1], velOut[1]); + + // Disabling velocities goes back to nullopt + joint.EnableVelocityCheck(ecm, false); + EXPECT_EQ(std::nullopt, joint.Velocity(ecm)); + EXPECT_EQ(nullptr, ecm.Component(eJoint)); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, Position) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // Before enabling, position should return nullopt + EXPECT_EQ(std::nullopt, joint.Position(ecm)); + + // After enabling, position should return default values + joint.EnablePositionCheck(ecm); + + EXPECT_NE(nullptr, ecm.Component(eJoint)); + + std::vector posOut = *joint.Position(ecm); + EXPECT_TRUE(posOut.empty()); + + // With custom positions + std::vector pos{-0.3, -0.4}; + ecm.SetComponentData(eJoint, pos); + posOut = *joint.Position(ecm); + EXPECT_EQ(2u, posOut.size()); + EXPECT_DOUBLE_EQ(pos[0], posOut[0]); + EXPECT_DOUBLE_EQ(pos[1], posOut[1]); + + // Disabling positions goes back to nullopt + joint.EnablePositionCheck(ecm, false); + EXPECT_EQ(std::nullopt, joint.Position(ecm)); + EXPECT_EQ(nullptr, ecm.Component(eJoint)); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, TransmittedWrench) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // Before enabling, wrench should return nullopt + EXPECT_EQ(std::nullopt, joint.TransmittedWrench(ecm)); + + // After enabling, wrench should return default values + joint.EnableTransmittedWrenchCheck(ecm); + + EXPECT_NE(nullptr, ecm.Component(eJoint)); + + std::vector wrenchOut = *joint.TransmittedWrench(ecm); + // todo(anyone) Unlike Velocity and Position functions that return an + // empty vector if it has not been populated yet, the wrench vector + // will contain one empty wrench msg. This is because the TransmittedWrench + // API workarounds the fact that the TransmittedWrench component contains + // only one wrench reading instead of a wrench vector like positions and + // velocities. + // EXPECT_TRUE(wrenchOut.empty()); + + // With custom wrench + msgs::Wrench wrenchMsg; + msgs::Set(wrenchMsg.mutable_force(), + math::Vector3d(0.2, 3.2, 0.1)); + + std::vector wrench{wrenchMsg}; + ecm.SetComponentData(eJoint, wrench[0]); + wrenchOut = *joint.TransmittedWrench(ecm); + EXPECT_EQ(1u, wrenchOut.size()); + EXPECT_DOUBLE_EQ(wrench[0].force().x(), wrenchOut[0].force().x()); + EXPECT_DOUBLE_EQ(wrench[0].force().y(), wrenchOut[0].force().y()); + EXPECT_DOUBLE_EQ(wrench[0].force().z(), wrenchOut[0].force().z()); + + // Disabling wrench goes back to nullopt + joint.EnableTransmittedWrenchCheck(ecm, false); + EXPECT_EQ(std::nullopt, joint.TransmittedWrench(ecm)); + EXPECT_EQ(nullptr, ecm.Component(eJoint)); +}