Joint Force-Torque Systems Plugin (#977)

Signed-off-by: Devansh <devansh@vt.edu> Signed-off-by: Michael Carroll <michael@openrobotics.org> Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org> Co-authored-by: Michael Carroll <michael@openrobotics.org> Co-authored-by: Addisu Z. Taddese <addisu@openrobotics.org>
gazebosim · Sep 22, 2021 · d889c5f · d889c5f
1 parent ae1a048
commit d889c5f
Show file tree

Hide file tree

Showing 10 changed files with 1,102 additions and 0 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -111,6 +111,7 @@ ign_find_package(ignition-sensors6 REQUIRED
     air_pressure
     altimeter
     imu
+    force_torque
     logical_camera
     magnetometer
 

diff --git a/include/ignition/gazebo/components/ForceTorque.hh b/include/ignition/gazebo/components/ForceTorque.hh
@@ -0,0 +1,47 @@
+/*
+ * 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_FORCETORQUE_HH_
+#define IGNITION_GAZEBO_COMPONENTS_FORCETORQUE_HH_
+
+#include <sdf/Sensor.hh>
+
+#include <ignition/gazebo/config.hh>
+#include <ignition/gazebo/Export.hh>
+
+#include <ignition/gazebo/components/Factory.hh>
+#include <ignition/gazebo/components/Component.hh>
+#include <ignition/gazebo/components/Serialization.hh>
+
+namespace ignition
+{
+namespace gazebo
+{
+// Inline bracket to help doxygen filtering.
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+namespace components
+{
+  /// \brief A component type that contains an FT sensor,
+  /// sdf::ForceTorque, information.
+  using ForceTorque = Component<sdf::Sensor, class ForceTorqueTag,
+      serializers::SensorSerializer>;
+  IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.ForceTorque",
+                                ForceTorque)
+}
+}
+}
+}
+#endif
diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc
@@ -35,6 +35,7 @@
 #include "ignition/gazebo/components/ContactSensor.hh"
 #include "ignition/gazebo/components/CustomSensor.hh"
 #include "ignition/gazebo/components/DepthCamera.hh"
+#include "ignition/gazebo/components/ForceTorque.hh"
 #include "ignition/gazebo/components/Geometry.hh"
 #include "ignition/gazebo/components/GpuLidar.hh"
 #include "ignition/gazebo/components/Gravity.hh"
@@ -598,6 +599,16 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Joint *_joint)
   this->dataPtr->ecm->CreateComponent(jointEntity,
       components::JointType(_joint->Type()));
 
+  // Sensors
+  for (uint64_t sensorIndex = 0; sensorIndex < _joint->SensorCount();
+      ++sensorIndex)
+  {
+    auto sensor = _joint->SensorByIndex(sensorIndex);
+    auto sensorEntity = this->CreateEntities(sensor);
+
+    this->SetParent(sensorEntity, jointEntity);
+  }
+
   if (_joint->Axis(0))
   {
     auto resolvedAxis = ResolveJointAxis(*_joint->Axis(0));
@@ -845,6 +856,11 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Sensor *_sensor)
     this->dataPtr->ecm->CreateComponent(sensorEntity,
             components::LinearAcceleration(math::Vector3d::Zero));
   }
+  else if (_sensor->Type() == sdf::SensorType::FORCE_TORQUE)
+  {
+    this->dataPtr->ecm->CreateComponent(sensorEntity,
+        components::ForceTorque(*_sensor));
+  }
   else if (_sensor->Type() == sdf::SensorType::LOGICAL_CAMERA)
   {
     auto elem = _sensor->Element();

diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt
@@ -99,6 +99,7 @@ add_subdirectory(camera_video_recorder)
 add_subdirectory(detachable_joint)
 add_subdirectory(diff_drive)
 add_subdirectory(follow_actor)
+add_subdirectory(force_torque)
 add_subdirectory(hydrodynamics)
 add_subdirectory(imu)
 add_subdirectory(joint_controller)

diff --git a/src/systems/force_torque/CMakeLists.txt b/src/systems/force_torque/CMakeLists.txt
@@ -0,0 +1,8 @@
+gz_add_system(forcetorque
+  SOURCES
+    ForceTorque.cc
+  PUBLIC_LINK_LIBS
+    ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER}
+  PRIVATE_LINK_LIBS
+    ignition-sensors${IGN_SENSORS_VER}::force_torque
+)