Add force torque sensor (#393)

Signed-off-by: Nick Lamprianidis <[email protected]>
Co-authored-by: Steve Peters <[email protected]>

include/sdf/CMakeLists.txt

@@ -15,6 +15,7 @@ set (headers
   Error.hh
   Exception.hh
   Filesystem.hh
+  ForceTorque.hh
   Frame.hh
   Geometry.hh
   Gui.hh

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 <string>
+#include <sdf/Error.hh>
+#include <sdf/Element.hh>
+#include <sdf/Noise.hh>
+#include <sdf/sdf_config.h>
+
+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

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);

sdf/1.4/sensor.sdf

@@ -33,13 +33,13 @@
   <include filename="plugin.sdf" required="*"/>
   <include filename="camera.sdf" required="0"/>
   <include filename="contact.sdf" required="0"/>
+  <include filename="forcetorque.sdf" required="0"/>
   <include filename="gps.sdf" required="0"/>
   <include filename="imu.sdf" required="0"/>
   <include filename="ray.sdf" required="0"/>
   <include filename="rfid.sdf" required="0"/>
   <include filename="rfidtag.sdf" required="0"/>
   <include filename="sonar.sdf" required="0"/>
   <include filename="transceiver.sdf" required="0"/>
-  <include filename="forcetorque.sdf" required="0"/>
 
 </element> <!-- End Sensor -->

sdf/1.5/forcetorque.sdf

@@ -13,7 +13,7 @@
   <element name="measure_direction" type="string" default="child_to_parent" required="0">
     <description>
       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.
     </description>
   </element>

sdf/1.5/sensor.sdf

@@ -49,6 +49,7 @@
   <include filename="altimeter.sdf" required="0"/>
   <include filename="camera.sdf" required="0"/>
   <include filename="contact.sdf" required="0"/>
+  <include filename="forcetorque.sdf" required="0"/>
   <include filename="gps.sdf" required="0"/>
   <include filename="imu.sdf" required="0"/>
   <include filename="logical_camera.sdf" required="0"/>
@@ -58,6 +59,5 @@
   <include filename="rfidtag.sdf" required="0"/>
   <include filename="sonar.sdf" required="0"/>
   <include filename="transceiver.sdf" required="0"/>
-  <include filename="forcetorque.sdf" required="0"/>
 
 </element> <!-- End Sensor -->

sdf/1.6/forcetorque.sdf

@@ -13,7 +13,7 @@
   <element name="measure_direction" type="string" default="child_to_parent" required="0">
     <description>
       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.
     </description>
   </element>

sdf/1.6/sensor.sdf

@@ -57,6 +57,7 @@
   <include filename="altimeter.sdf" required="0"/>
   <include filename="camera.sdf" required="0"/>
   <include filename="contact.sdf" required="0"/>
+  <include filename="forcetorque.sdf" required="0"/>
   <include filename="gps.sdf" required="0"/>
   <include filename="imu.sdf" required="0"/>
   <include filename="lidar.sdf" required="0"/>
@@ -67,6 +68,5 @@
   <include filename="rfidtag.sdf" required="0"/>
   <include filename="sonar.sdf" required="0"/>
   <include filename="transceiver.sdf" required="0"/>
-  <include filename="forcetorque.sdf" required="0"/>
 
 </element> <!-- End Sensor -->

sdf/1.7/forcetorque.sdf

@@ -13,7 +13,7 @@
   <element name="measure_direction" type="string" default="child_to_parent" required="0">
     <description>
       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.
     </description>
   </element>

sdf/1.7/sensor.sdf

@@ -56,6 +56,7 @@
   <include filename="altimeter.sdf" required="0"/>
   <include filename="camera.sdf" required="0"/>
   <include filename="contact.sdf" required="0"/>
+  <include filename="forcetorque.sdf" required="0"/>
   <include filename="gps.sdf" required="0"/>
   <include filename="imu.sdf" required="0"/>
   <include filename="lidar.sdf" required="0"/>
@@ -66,6 +67,5 @@
   <include filename="rfidtag.sdf" required="0"/>
   <include filename="sonar.sdf" required="0"/>
   <include filename="transceiver.sdf" required="0"/>
-  <include filename="forcetorque.sdf" required="0"/>
 
 </element> <!-- End Sensor -->

sdf/1.8/forcetorque.sdf

@@ -13,7 +13,7 @@
   <element name="measure_direction" type="string" default="child_to_parent" required="0">
     <description>
       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.
     </description>
   </element>

sdf/1.8/sensor.sdf

@@ -56,6 +56,7 @@
   <include filename="altimeter.sdf" required="0"/>
   <include filename="camera.sdf" required="0"/>
   <include filename="contact.sdf" required="0"/>
+  <include filename="forcetorque.sdf" required="0"/>
   <include filename="gps.sdf" required="0"/>
   <include filename="imu.sdf" required="0"/>
   <include filename="lidar.sdf" required="0"/>
@@ -66,6 +67,5 @@
   <include filename="rfidtag.sdf" required="0"/>
   <include filename="sonar.sdf" required="0"/>
   <include filename="transceiver.sdf" required="0"/>
-  <include filename="forcetorque.sdf" required="0"/>
 
 </element> <!-- End Sensor -->

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