diff --git a/include/sdf/CMakeLists.txt b/include/sdf/CMakeLists.txt index ed0930a5a..7b096e901 100644 --- a/include/sdf/CMakeLists.txt +++ b/include/sdf/CMakeLists.txt @@ -15,6 +15,7 @@ set (headers Error.hh Exception.hh Filesystem.hh + ForceTorque.hh Frame.hh Geometry.hh Gui.hh diff --git a/include/sdf/ForceTorque.hh b/include/sdf/ForceTorque.hh new file mode 100644 index 000000000..2a68aaa31 --- /dev/null +++ b/include/sdf/ForceTorque.hh @@ -0,0 +1,139 @@ +/* + * Copyright 2020 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 SDF_FORCE_TORQUE_HH_ +#define SDF_FORCE_TORQUE_HH_ + +#include +#include +#include +#include +#include + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // Forward declare private data class. + class ForceTorquePrivate; + + /// \enum ForceTorqueFrame + /// \brief The set of supported frames of the wrench values. + enum class ForceTorqueFrame : uint8_t + { + /// \brief Invalid frame + INVALID = 0, + + /// \brief Wrench expressed in the orientation of the parent link frame + PARENT = 1, + + /// \brief Wrench expressed in the orientation of the child link frame + CHILD = 2, + + /// \brief Wrench expressed in the orientation of the joint sensor frame + SENSOR = 3 + }; + + /// \enum ForceTorqueMeasureDirection + /// \brief The set of measure directions of the wrench values. + enum class ForceTorqueMeasureDirection : uint8_t + { + /// \brief Invalid frame + INVALID = 0, + + /// \brief Wrench measured as applied by the parent link on the child link + PARENT_TO_CHILD = 1, + + /// \brief Wrench measured as applied by the child link on the parent link + CHILD_TO_PARENT = 2 + }; + + /// \brief ForceTorque contains information about a force torque sensor. + /// This sensor can be attached to a joint. + class SDFORMAT_VISIBLE ForceTorque + { + /// \brief Default constructor + public: ForceTorque(); + + /// \brief Copy constructor + /// \param[in] _ft The force torque sensor to copy. + public: ForceTorque(const ForceTorque &_ft); + + /// \brief Move constructor + /// \param[in] _ft The force torque sensor to move. + public: ForceTorque(ForceTorque &&_ft) noexcept; + + /// \brief Destructor + public: ~ForceTorque(); + + /// \brief Assignment operator. + /// \param[in] _ft The force torque sensor to set values from. + /// \return *this + public: ForceTorque &operator=(const ForceTorque &_ft); + + /// \brief Move assignment operator. + /// \param[in] _ft The force torque sensor to set values from. + /// \return *this + public: ForceTorque &operator=(ForceTorque &&_ft) noexcept; + + /// \brief Load the force torque sensor based on an element pointer. This is + /// *not* the usual entry point. Typical usage of the SDF DOM is through the + /// Root object. + /// \param[in] _sdf The SDF Element pointer + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(ElementPtr _sdf); + + /// \brief Get a pointer to the SDF element that was used during load. + /// \return SDF element pointer. The value will be nullptr if Load has + /// not been called. + public: sdf::ElementPtr Element() const; + + /// \brief Get the frame in which the wrench values are reported. + /// \return The frame of the wrench values. + public: ForceTorqueFrame Frame() const; + + /// \brief Set the frame in which the wrench values are reported. + /// \param[in] _frame The frame of the wrench values. + public: void SetFrame(ForceTorqueFrame _frame) const; + + /// \brief Get the measure direction of the wrench values. + /// \return The measure direction of the wrench values. + public: ForceTorqueMeasureDirection MeasureDirection() const; + + /// \brief Set the measure direction of the wrench values. + /// \param[in] _direction The measure direction of the wrench values. + public: void SetMeasureDirection( + ForceTorqueMeasureDirection _direction) const; + + /// \brief Return true if both force torque objects contain the same values. + /// \param[_in] _ft Force torque value to compare. + /// \returen True if 'this' == _ft. + public: bool operator==(const ForceTorque &_ft) const; + + /// \brief Return true this force torque object does not contain the same + /// values as the passed-in parameter. + /// \param[_in] _ft Force torque value to compare. + /// \returen True if 'this' != _ft. + public: bool operator!=(const ForceTorque &_ft) const; + + /// \brief Private data pointer. + private: ForceTorquePrivate *dataPtr; + }; + } +} + +#endif diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index b5b92bd7d..1b5f04986 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -36,6 +36,7 @@ namespace sdf class AirPressure; class Altimeter; class Camera; + class ForceTorque; class Imu; class Lidar; class Magnetometer; @@ -289,6 +290,17 @@ namespace sdf /// \sa SensorType Type() const public: const Camera *CameraSensor() const; + /// \brief Set the force torque sensor. + /// \param[in] _ft The force torque sensor. + public: void SetForceTorqueSensor(const ForceTorque &_ft); + + /// \brief Get a pointer to a force torque sensor, or nullptr if the sensor + /// does not contain a force torque sensor. + /// \return Pointer to the force torque sensor, or nullptr if the sensor + /// is not a force torque sensor. + /// \sa SensorType Type() const + public: const ForceTorque *ForceTorqueSensor() const; + /// \brief Set the IMU sensor. /// \param[in] _imu The IMU sensor. public: void SetImuSensor(const Imu &_imu); diff --git a/sdf/1.4/sensor.sdf b/sdf/1.4/sensor.sdf index df4749333..1d815338e 100644 --- a/sdf/1.4/sensor.sdf +++ b/sdf/1.4/sensor.sdf @@ -33,6 +33,7 @@ + @@ -40,6 +41,5 @@ - diff --git a/sdf/1.5/forcetorque.sdf b/sdf/1.5/forcetorque.sdf index 4fd5e742b..18e5b8f71 100644 --- a/sdf/1.5/forcetorque.sdf +++ b/sdf/1.5/forcetorque.sdf @@ -13,7 +13,7 @@ Direction of the wrench measured by the sensor. The supported options are: - "parent_to_child" if the measured wrench is the one applied by parent link on the child link, + "parent_to_child" if the measured wrench is the one applied by the parent link on the child link, "child_to_parent" if the measured wrench is the one applied by the child link on the parent link. diff --git a/sdf/1.5/sensor.sdf b/sdf/1.5/sensor.sdf index 5891cfa7f..e58ae79b3 100644 --- a/sdf/1.5/sensor.sdf +++ b/sdf/1.5/sensor.sdf @@ -49,6 +49,7 @@ + @@ -58,6 +59,5 @@ - diff --git a/sdf/1.6/forcetorque.sdf b/sdf/1.6/forcetorque.sdf index 4fd5e742b..18e5b8f71 100644 --- a/sdf/1.6/forcetorque.sdf +++ b/sdf/1.6/forcetorque.sdf @@ -13,7 +13,7 @@ Direction of the wrench measured by the sensor. The supported options are: - "parent_to_child" if the measured wrench is the one applied by parent link on the child link, + "parent_to_child" if the measured wrench is the one applied by the parent link on the child link, "child_to_parent" if the measured wrench is the one applied by the child link on the parent link. diff --git a/sdf/1.6/sensor.sdf b/sdf/1.6/sensor.sdf index eb8cff4e7..978522289 100644 --- a/sdf/1.6/sensor.sdf +++ b/sdf/1.6/sensor.sdf @@ -57,6 +57,7 @@ + @@ -67,6 +68,5 @@ - diff --git a/sdf/1.7/forcetorque.sdf b/sdf/1.7/forcetorque.sdf index 4fd5e742b..18e5b8f71 100644 --- a/sdf/1.7/forcetorque.sdf +++ b/sdf/1.7/forcetorque.sdf @@ -13,7 +13,7 @@ Direction of the wrench measured by the sensor. The supported options are: - "parent_to_child" if the measured wrench is the one applied by parent link on the child link, + "parent_to_child" if the measured wrench is the one applied by the parent link on the child link, "child_to_parent" if the measured wrench is the one applied by the child link on the parent link. diff --git a/sdf/1.7/sensor.sdf b/sdf/1.7/sensor.sdf index d5f8c938b..0944cd333 100644 --- a/sdf/1.7/sensor.sdf +++ b/sdf/1.7/sensor.sdf @@ -56,6 +56,7 @@ + @@ -66,6 +67,5 @@ - diff --git a/sdf/1.8/forcetorque.sdf b/sdf/1.8/forcetorque.sdf index 4fd5e742b..18e5b8f71 100644 --- a/sdf/1.8/forcetorque.sdf +++ b/sdf/1.8/forcetorque.sdf @@ -13,7 +13,7 @@ Direction of the wrench measured by the sensor. The supported options are: - "parent_to_child" if the measured wrench is the one applied by parent link on the child link, + "parent_to_child" if the measured wrench is the one applied by the parent link on the child link, "child_to_parent" if the measured wrench is the one applied by the child link on the parent link. diff --git a/sdf/1.8/sensor.sdf b/sdf/1.8/sensor.sdf index d5f8c938b..0944cd333 100644 --- a/sdf/1.8/sensor.sdf +++ b/sdf/1.8/sensor.sdf @@ -56,6 +56,7 @@ + @@ -66,6 +67,5 @@ - diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 5df95ae35..4625a432e 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -26,6 +26,7 @@ set (sources Frame.cc FrameSemantics.cc Filesystem.cc + ForceTorque.cc Geometry.cc Gui.cc ign.cc @@ -93,6 +94,7 @@ if (BUILD_SDF_TEST) Exception_TEST.cc Frame_TEST.cc Filesystem_TEST.cc + ForceTorque_TEST.cc Geometry_TEST.cc Gui_TEST.cc Imu_TEST.cc diff --git a/src/ForceTorque.cc b/src/ForceTorque.cc new file mode 100644 index 000000000..8a467d5c4 --- /dev/null +++ b/src/ForceTorque.cc @@ -0,0 +1,186 @@ +/* + * Copyright 2020 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 "sdf/ForceTorque.hh" + +using namespace sdf; + +/// \brief Private force torque data. +class sdf::ForceTorquePrivate +{ + /// \brief Name of the reference frame for the wrench values. + public: ForceTorqueFrame frame = ForceTorqueFrame::CHILD; + + /// \brief The direction of the measured wrench values. + public: ForceTorqueMeasureDirection measure_direction = + ForceTorqueMeasureDirection::CHILD_TO_PARENT; + + /// \brief The SDF element pointer used during load. + public: sdf::ElementPtr sdf; +}; + +////////////////////////////////////////////////// +ForceTorque::ForceTorque() + : dataPtr(new ForceTorquePrivate) +{ +} + +////////////////////////////////////////////////// +ForceTorque::~ForceTorque() +{ + delete this->dataPtr; + this->dataPtr = nullptr; +} + +////////////////////////////////////////////////// +ForceTorque::ForceTorque(const ForceTorque &_ft) + : dataPtr(new ForceTorquePrivate(*_ft.dataPtr)) +{ +} + +////////////////////////////////////////////////// +ForceTorque::ForceTorque(ForceTorque &&_ft) noexcept + : dataPtr(std::exchange(_ft.dataPtr, nullptr)) +{ +} + +////////////////////////////////////////////////// +ForceTorque &ForceTorque::operator=(const ForceTorque &_ft) +{ + return *this = ForceTorque(_ft); +} + +////////////////////////////////////////////////// +ForceTorque &ForceTorque::operator=(ForceTorque &&_ft) noexcept +{ + std::swap(this->dataPtr, _ft.dataPtr); + return *this; +} + +////////////////////////////////////////////////// +Errors ForceTorque::Load(ElementPtr _sdf) +{ + Errors errors; + + this->dataPtr->sdf = _sdf; + + // Check that the provided SDF element is a element. + // This is an error that cannot be recovered, so return an error. + if (_sdf->GetName() != "force_torque") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load a force torque sensor, but the provided SDF " + "element is not a ."}); + return errors; + } + + if (_sdf->HasElement("frame")) + { + std::string frame = _sdf->Get("frame", "child").first; + + if (frame == "parent") + { + this->dataPtr->frame = ForceTorqueFrame::PARENT; + } + else if (frame == "child") + { + this->dataPtr->frame = ForceTorqueFrame::CHILD; + } + else if (frame == "sensor") + { + this->dataPtr->frame = ForceTorqueFrame::SENSOR; + } + else + { + this->dataPtr->frame = ForceTorqueFrame::INVALID; + errors.push_back({ErrorCode::ELEMENT_INVALID, + "ForceTorque element 'frame' is invalid with a value of [" + frame + + "]. Refer to the SDF documentation for the list of valid frames"}); + } + } + + if (_sdf->HasElement("measure_direction")) + { + std::string direction = + _sdf->Get("measure_direction", "child_to_parent").first; + + if (direction == "parent_to_child") + { + this->dataPtr->measure_direction = + ForceTorqueMeasureDirection::PARENT_TO_CHILD; + } + else if (direction == "child_to_parent") + { + this->dataPtr->measure_direction = + ForceTorqueMeasureDirection::CHILD_TO_PARENT; + } + else + { + this->dataPtr->measure_direction = ForceTorqueMeasureDirection::INVALID; + errors.push_back({ErrorCode::ELEMENT_INVALID, + "ForceTorque element 'measure_direction' is invalid with a value " + "of [" + direction + "]. Refer to the SDF documentation for the " + "list of valid frames"}); + } + } + + return errors; +} + +////////////////////////////////////////////////// +sdf::ElementPtr ForceTorque::Element() const +{ + return this->dataPtr->sdf; +} + +////////////////////////////////////////////////// +bool ForceTorque::operator!=(const ForceTorque &_ft) const +{ + return !(*this == _ft); +} + +////////////////////////////////////////////////// +bool ForceTorque::operator==(const ForceTorque &_ft) const +{ + return this->dataPtr->frame == _ft.dataPtr->frame && + this->dataPtr->measure_direction == _ft.dataPtr->measure_direction; +} + +////////////////////////////////////////////////// +ForceTorqueFrame ForceTorque::Frame() const +{ + return this->dataPtr->frame; +} + +////////////////////////////////////////////////// +void ForceTorque::SetFrame(ForceTorqueFrame _frame) const +{ + this->dataPtr->frame = _frame; +} + +////////////////////////////////////////////////// +ForceTorqueMeasureDirection ForceTorque::MeasureDirection() const +{ + return this->dataPtr->measure_direction; +} + +////////////////////////////////////////////////// +void ForceTorque::SetMeasureDirection( + ForceTorqueMeasureDirection _direction) const +{ + this->dataPtr->measure_direction = _direction; +} diff --git a/src/ForceTorque_TEST.cc b/src/ForceTorque_TEST.cc new file mode 100644 index 000000000..57bff24f8 --- /dev/null +++ b/src/ForceTorque_TEST.cc @@ -0,0 +1,79 @@ +/* + * Copyright (C) 2020 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 "sdf/ForceTorque.hh" + +///////////////////////////////////////////////// +TEST(DOMForceTorque, Construction) +{ + sdf::ForceTorque ft; + + EXPECT_EQ(ft.Frame(), sdf::ForceTorqueFrame::CHILD); + ft.SetFrame(sdf::ForceTorqueFrame::PARENT); + EXPECT_EQ(ft.Frame(), sdf::ForceTorqueFrame::PARENT); + + EXPECT_EQ(ft.MeasureDirection(), + sdf::ForceTorqueMeasureDirection::CHILD_TO_PARENT); + ft.SetMeasureDirection(sdf::ForceTorqueMeasureDirection::PARENT_TO_CHILD); + EXPECT_EQ(ft.MeasureDirection(), + sdf::ForceTorqueMeasureDirection::PARENT_TO_CHILD); + + // Copy constructor + sdf::ForceTorque ft2(ft); + EXPECT_EQ(ft2, ft); + + // Copy operator + sdf::ForceTorque ft3; + ft3 = ft; + EXPECT_EQ(ft3, ft); + + // Move constructor + sdf::ForceTorque ft4(std::move(ft)); + EXPECT_EQ(ft4, ft2); + ft = ft4; + EXPECT_EQ(ft, ft2); + + // Move operator + sdf::ForceTorque ft5; + ft5 = std::move(ft2); + EXPECT_EQ(ft5, ft3); + ft2 = ft5; + EXPECT_EQ(ft2, ft3); + + // Inequality + sdf::ForceTorque ft6; + EXPECT_NE(ft6, ft3); +} + +///////////////////////////////////////////////// +TEST(DOMForceTorque, Load) +{ + sdf::ElementPtr sdf(std::make_shared()); + + sdf::ForceTorque ft; + sdf::Errors errors = ft.Load(sdf); + EXPECT_FALSE(errors.empty()); + EXPECT_TRUE(errors[0].Message().find("is not a ") + != std::string::npos) << errors[0].Message(); + + EXPECT_NE(nullptr, ft.Element()); + EXPECT_EQ(sdf.get(), ft.Element().get()); + + // The ForceTorque::Load function is tested more thouroughly in the + // link_dom.cc integration test. +} diff --git a/src/Sensor.cc b/src/Sensor.cc index 60f892431..842ceae32 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -22,6 +22,7 @@ #include "sdf/Altimeter.hh" #include "sdf/Camera.hh" #include "sdf/Error.hh" +#include "sdf/ForceTorque.hh" #include "sdf/Imu.hh" #include "sdf/Magnetometer.hh" #include "sdf/Lidar.hh" @@ -94,6 +95,11 @@ class sdf::SensorPrivate { this->camera = std::make_unique(*_sensor.camera); } + if (_sensor.forceTorque) + { + this->forceTorque = std::make_unique( + *_sensor.forceTorque); + } if (_sensor.imu) { this->imu = std::make_unique(*_sensor.imu); @@ -145,6 +151,9 @@ class sdf::SensorPrivate /// \brief Pointer to a camera. public: std::unique_ptr camera; + /// \brief Pointer to a force torque sensor. + public: std::unique_ptr forceTorque; + /// \brief Pointer to an IMU. public: std::unique_ptr imu; @@ -221,6 +230,8 @@ bool Sensor::operator==(const Sensor &_sensor) const return *(this->dataPtr->magnetometer) == *(_sensor.dataPtr->magnetometer); case SensorType::AIR_PRESSURE: return *(this->dataPtr->airPressure) == *(_sensor.dataPtr->airPressure); + case SensorType::FORCE_TORQUE: + return *(this->dataPtr->forceTorque) == *(_sensor.dataPtr->forceTorque); case SensorType::IMU: return *(this->dataPtr->imu) == *(_sensor.dataPtr->imu); case SensorType::CAMERA: @@ -341,6 +352,10 @@ Errors Sensor::Load(ElementPtr _sdf) else if (type == "force_torque") { this->dataPtr->type = SensorType::FORCE_TORQUE; + this->dataPtr->forceTorque.reset(new ForceTorque()); + Errors err = this->dataPtr->forceTorque->Load( + _sdf->GetElement("force_torque")); + errors.insert(errors.end(), err.begin(), err.end()); } else if (type == "gps") { @@ -356,7 +371,6 @@ Errors Sensor::Load(ElementPtr _sdf) } else if (type == "imu") { - this->dataPtr->type = SensorType::IMU; this->dataPtr->type = SensorType::IMU; this->dataPtr->imu.reset(new Imu()); Errors err = this->dataPtr->imu->Load(_sdf->GetElement("imu")); @@ -604,6 +618,18 @@ const Camera *Sensor::CameraSensor() const return this->dataPtr->camera.get(); } +///////////////////////////////////////////////// +void Sensor::SetForceTorqueSensor(const ForceTorque &_ft) +{ + this->dataPtr->forceTorque = std::make_unique(_ft); +} + +///////////////////////////////////////////////// +const ForceTorque *Sensor::ForceTorqueSensor() const +{ + return this->dataPtr->forceTorque.get(); +} + ///////////////////////////////////////////////// void Sensor::SetImuSensor(const Imu &_imu) {