Add --dextrorotary config option to Jr3

roboticslab-uc3m · Jun 29, 2022 · 6cb1f8e · 6cb1f8e
1 parent 09d31ca
commit 6cb1f8e
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 0 deletions.
diff --git a/libraries/YarpPlugins/Jr3/DeviceDriverImpl.cpp b/libraries/YarpPlugins/Jr3/DeviceDriverImpl.cpp
@@ -68,6 +68,19 @@ bool Jr3::open(yarp::os::Searchable& config)
 
     loadFilters(filterId);
 
+    yCInfo(JR3) << "Using filter ID:" << filterId;
+
+    isDextrorotary = config.check("dextrorotary", "assume dextrorotary XYZ coordinates");
+
+    if (isDextrorotary)
+    {
+        yCInfo(JR3) << "Assuming dextrorotary XYZ coordinates";
+    }
+    else
+    {
+        yCInfo(JR3) << "Assuming levorotary XYZ coordinates";
+    }
+
     if ((fd = ::open("/dev/jr3", O_RDWR)) < 0)
     {
         yCError(JR3) << "Can't open device, no way to read force!";

diff --git a/libraries/YarpPlugins/Jr3/ISixAxisForceTorqueSensorsImpl.cpp b/libraries/YarpPlugins/Jr3/ISixAxisForceTorqueSensorsImpl.cpp
@@ -70,6 +70,12 @@ bool Jr3::getSixAxisForceTorqueSensorMeasure(std::size_t sens_index, yarp::sig::
     out[4] = fm.m[1] * fs[sens_index].m[1] * factor;
     out[5] = fm.m[2] * fs[sens_index].m[2] * factor;
 
+    if (isDextrorotary)
+    {
+        out[0] = -out[0];
+        out[3] = -out[3];
+    }
+
     timestamp = yarp::os::SystemClock::nowSystem();
 
     return true;

diff --git a/libraries/YarpPlugins/Jr3/Jr3.hpp b/libraries/YarpPlugins/Jr3/Jr3.hpp
@@ -103,6 +103,7 @@ class Jr3 : public yarp::dev::DeviceDriver,
     force_array fs[4];
     unsigned long int filters[4];
     std::vector<std::string> names {"ch0", "ch1", "ch2", "ch3"};
+    bool isDextrorotary {false};
 };
 
 } // namespace roboticslab